#include "AttitudeSolution.h"

extern float dt;
//平方根倒数
float norm = 1;

void Mahony(Mpu6500_DataTydef *mpu_data, float gx, float gy, float gz, float ax, float ay, float az)
{

    float vx = 0, vy = 0, vz = 0; //实际重力加速度
    float ex = 0, ey = 0, ez = 0; //叉积计算的误差

    if (ax * ay * az == 0)
        return;
    /*
        //防止四元数锁死
          if (fabs(q0) > 2 || fabs(q1) > 2 || fabs(q2) > 2 || fabs(q3) > 2)
        {
            q0 = 1;
            q1 = 0;
            q2 = 0;
            q3 = 0;
            exInt = 0;
            eyInt = 0;
            ezInt = 0;
        }
    */
    //加速度计测量的重力方向(机体坐标系)

    norm = MyInvSqrt(ax * ax + ay * ay + az * az);
    ax *= norm;
    ay *= norm;
    az *= norm;

    //四元数推出的实际重力方向(机体坐标系)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    //叉积误差
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    //叉积误差积分为角速度
    exInt += ex * Ki;
    eyInt += ey * Ki;
    ezInt += ez * Ki;

    //角速度补偿
    gx += Kp * ex + exInt;
    gy += Kp * ey + eyInt;
    gz += Kp * ez + ezInt;

    //更新四元数
    float t0 = q0;
    float t1 = q1;
    float t2 = q2;
    float t3 = q3;

    q0 += (-t0 * gx - t2 * gy - t3 * gz) * halfT;
    q1 += (t0 * gx + t2 * gz - t3 * gy) * halfT;
    q2 += (t0 * gy - t1 * gz + t3 * gx) * halfT;
    q3 += (t0 * gz + t1 * gy - t2 * gx) * halfT;

    //单位化四元数
    norm = MyInvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= norm;
    q1 *= norm;
    q2 *= norm;
    q3 *= norm;

    //四元数反解欧拉角
    mpu_data->Yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);
    mpu_data->Pitch = -asin(2.f * (q1q3 - q0q2));
    mpu_data->Roll = atan2(2.f * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3);
}
